//
// Created by jiang on 2021/12/15.
//

#include "current.h"
#include "motor.h"
#include "pwm.h"
#include "bus.h"
#include "foc.h"
#include "pid.h"
#include "filter.h"

motor_t motor;
ERR_State_t ERR_State;


void MOTO_Init(motor_t *pHandle) {
    /* parameter config */
    pHandle->ready = 0;
    pHandle->enable = 0;
    pHandle->setRpm = 0;
    pHandle->eSpeed = 0;

    pHandle->dq_dest.d = 0;
    pHandle->dq_dest.q = 200;

    PID_Init(&Idpi, 75, 250);
    PID_SetLimitIntegral(&Idpi, 900);
    PID_SetLimitOutput(&Idpi, 900);

    PID_Init(&Iqpi, 75, 250);
    PID_SetLimitIntegral(&Iqpi, 900);
    PID_SetLimitOutput(&Iqpi, 900);


#if defined(USE_OUT_FILTER_LPF)
    LPF_Init(&pHandle->lpfd, 500);
    LPF_Init(&pHandle->lpfq, 500);

#elif defined(USE_OUT_FILTER_KALMAN)
    Kalman_Init(&pHandle->klmd, 250, 7400);
    Kalman_Init(&pHandle->klmq, 250, 8380);

#elif defined(USE_OUT_FILTER_IIR)
    IIR_Init(&pHandle->irrd, NUM, DEN);
    IIR_Init(&pHandle->irrq, NUM, DEN);

#endif

    CURR_Init(&current);

    PWM_Init(&pwm);
    BUS_Init(&bus);


    /* parameter config */
    CURR_Calibration(&current);

    pHandle->ready = 1;
}


void MOTO_Process(motor_t *pHandle) {
    if (pHandle->ready && pHandle->enable && ERR_State.all == 0) {

        pHandle->theta += pHandle->eSpeed;
        pHandle->dq_dest.q = (uint16_t) (pHandle->setRpm * 0.3f + 300);
        pHandle->dq_dest.q = (pHandle->dq_dest.q > 900) ? (900) : (pHandle->dq_dest.q);
//        BUS_Update(&bus, &adc);
        CURR_GetFromAdcVal(&current, &adc);

        FOC_Clarke(&(current.phase), &pHandle->ap_curr);

        FOC_Park(&pHandle->ap_curr, pHandle->theta, &pHandle->dq_curr);

        pHandle->dq_err.d = pHandle->dq_dest.d - pHandle->dq_curr.d;
        pHandle->dq_err.q = pHandle->dq_dest.q - pHandle->dq_curr.q;

        pHandle->dq_out.d = PID_Process(&Idpi, pHandle->dq_err.d);
        pHandle->dq_out.q = PID_Process(&Iqpi, pHandle->dq_err.q);

//#if defined(USE_OUT_FILTER_LPF)
//        pHandle->dq_filter.d = LPF_Filter(&pHandle->lpfd, pHandle->dq_out.d, pHandle->dq_filter.d);
//        pHandle->dq_filter.q = LPF_Filter(&pHandle->lpfq, pHandle->dq_out.q, pHandle->dq_filter.q);
//
//#elif defined(USE_OUT_FILTER_KALMAN)
//        pHandle->dq_filter.d = Kalman_Filter(&pHandle->klmd, pHandle->dq_out.d, pHandle->dq_filter.d);
//        pHandle->dq_filter.q = Kalman_Filter(&pHandle->klmq, pHandle->dq_out.q, pHandle->dq_filter.q);
//
//#elif defined(USE_OUT_FILTER_IIR)
//        pHandle->dq_filter.d = IIR_Filter(&pHandle->iird, pHandle->dq_out.d);
//        pHandle->dq_filter.q = IIR_Filter(&pHandle->iirq, pHandle->dq_out.q);
//
//#endif


        FOC_InvPark(&pHandle->dq_out, &pHandle->ap_volt);

//        pHandle->dq_test.d = PID_Process(&Idpi, pHandle->dq_err.d);
//        pHandle->dq_test.q = PID_Process(&Iqpi, pHandle->dq_err.q);
//        pHandle->dq_out.d = 0;
//        pHandle->dq_out.q = 300;
//        FOC_InvParkWithAngle(&pHandle->dq_out, pHandle->theta, &pHandle->ap_volt);

        pHandle->sector = FOC_SVPWM(&pHandle->ap_volt, &pHandle->t_phase);

        PWM_SetCompare(&pwm, &pHandle->t_phase);

    } else if (ERR_State.all && pHandle->enable) {
        pHandle->enable = 0;
        PWM_DisableOutput(&pwm);
    }
}

void MOTO_SpeedControl(motor_t *pHandle, int16_t rpmCtrl) {
    if (pHandle->setRpm < 0)
        pHandle->setRpm = 0;
    else if (pHandle->setRpm > 3000)
        pHandle->setRpm = 3000;

    pHandle->setRpm += rpmCtrl;
//    pHandle->eSpeed = (int16_t)((uint32_t)(pHandle->setRpm) * M2_PI * MOTO_POLE_COUNT / ((uint32_t)60 * PWM_FREQ));
    pHandle->eSpeed = (int16_t) ((uint32_t) (pHandle->setRpm) * M_PI / 60000);

    if (!pHandle->enable && pHandle->setRpm > 0 && pHandle->ready) {
        pHandle->enable = 1;
        PWM_EnableOutput(&pwm);
    }

    if (pHandle->enable && (pHandle->setRpm == 0)) {
        pHandle->enable = 0;
        PWM_DisableOutput(&pwm);
    }
}


void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
    if (GPIO_Pin == MFAULT_Pin) {
        if (GPIO_PIN_RESET == HAL_GPIO_ReadPin(MFAULT_GPIO_Port, MFAULT_Pin)) {
            ERR_State.bits.drv_fault = 1;
        }
    }
}


